/*****************************************************************************
 *   +--+
 *   | ++----+   
 *   +-++    |  
 *     |     |  
 *   +-+--+  |   
 *   | +--+--+  
 *   +----+    Copyright (c) 2009-13 Code Red Technologies Ltd.
 *
 * LPC21xx Startup code for use with Red Suite
 *
 * Version : 130903
 *
 * Software License Agreement
 * 
 * The software is owned by Code Red Technologies and/or its suppliers, and is 
 * protected under applicable copyright laws.  All rights are reserved.  Any 
 * use in violation of the foregoing restrictions may subject the user to criminal 
 * sanctions under applicable laws, as well as to civil liability for the breach 
 * of the terms and conditions of this license.
 * 
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * USE OF THIS SOFTWARE FOR COMMERCIAL DEVELOPMENT AND/OR EDUCATION IS SUBJECT
 * TO A CURRENT END USER LICENSE AGREEMENT (COMMERCIAL OR EDUCATIONAL) WITH
 * CODE RED TECHNOLOGIES LTD. 
 *
 ******************************************************************************/

/*******************************************************************************
 * 
 * Conventions used in the assembler code:
 * 
 * Mnemonics are in upper case.
 * labels (prog or data) are in lower case.
 * Constants for initialization values and/or conditional assembly are
 * in upper case.
 *
 * The following C preprocessor 'switches' are used in this file to conditionally
 * assemble certain parts of the code:
 *
 * PLL_INIT    : Define this to initialise the PLL
 * VPB_INIT    : Define this to initialise the VPB divider
 * MAM_INIT    : Define this to initialise the MAM
 * STACK_INIT  : Define this to initialise the STACK
 * USB_INIT    : Define this to enable USB (makes 8KB of USB RAM available
 *                                          on LPC2146/2148)
 * USE_OLD_STYLE_DATA_BSS_INIT : Define this to build startup code with pre-3.6
 *               version of the Code Red tools
 *
 *******************************************************************************/

        .global main                    // int main(void)

#ifndef USE_OLD_STYLE_DATA_BSS_INIT
/*****************************************************************************
/ The following symbols are constructs generated by the linker, indicating
/ the location of various points in the "Global Section Table". This table is
/ created by the linker via the Code Red managed linker script mechanism. It
/ contains the load address, execution address and length of each RW data
/ section and the execution and length of each BSS (zero initialized) section.
/*****************************************************************************/
		.global __data_section_table
        .global __data_section_table_end
        .global __bss_section_table
        .global __bss_section_table_end
#else
/*****************************************************************************
/ The following symbols are constructs generated by the linker, indicating
/ the load address, execution address and length of the RW data section and
/ the execution and length of the BSS (zero initialized) section.
/ Note that these symbols are not normally used by the managed linker script
/ mechanism in Red Suite/LPCXpresso 3.6 (Windows) and LPCXpresso 3.8 (Linux).
/ They are provide here simply so this startup code can be used with earlier
/ versions of Red Suite which do not support the more advanced managed linker
/ script mechanism introduced in the above version. To enable their use,
/ define "USE_OLD_STYLE_DATA_BSS_INIT".
/*****************************************************************************/
		.global _etext
		.global _data
		.global _edata
		.global _bss
		.global _ebss;
#endif

        .global _vStackTop              // top of stack

/*
 * Stack Sizes - remember these are bytes not words
 */
        .set  UND_STACK_SIZE, 0x00000010
        .set  ABT_STACK_SIZE, 0x00000010
        .set  FIQ_STACK_SIZE, 0x00000010
        .set  IRQ_STACK_SIZE, 0X00000080
        .set  SVC_STACK_SIZE, 0x00000080

        .set  USR_STACK_SIZE, 0x00000400 // Shared with SYS mode
        
        .set TOTAL_STACK_SIZE, UND_STACK_SIZE + SVC_STACK_SIZE + \
                                ABT_STACK_SIZE + FIQ_STACK_SIZE + \
                                IRQ_STACK_SIZE + USR_STACK_SIZE

/*
 * Standard definitions of Mode bits and Interrupt (I & F) flags in PSRs
 */
        .set  MODE_USR, 0x10            // User Mode
        .set  MODE_FIQ, 0x11            // FIQ Mode
        .set  MODE_IRQ, 0x12            // IRQ Mode
        .set  MODE_SVC, 0x13            // Supervisor Mode
        .set  MODE_ABT, 0x17            // Abort Mode
        .set  MODE_UND, 0x1B            // Undefined Mode
        .set  MODE_SYS, 0x1F            // System Mode

        .set  I_BIT, 0x80               // when I bit is set, IRQ is disabled
        .set  F_BIT, 0x40               // when F bit is set, FIQ is disabled

        .text
        .code 32
        .align 2

		.section .isr_vector,"x"
        .global _boot
        .func   _boot
_boot:

/*
 * Exception Processing Vectors
 */
Vectors:
        B     _start                    // reset
        MOV   pc,#__undf                  // undefined
        MOV   pc,#__swi                   // SWI/SVC
        MOV   pc,#__pabt                  // program abort
        MOV   pc,#__dabt                  // data abort
        NOP                             // Reserved for the flash checksum
        LDR   pc,[pc,#-0xFF0]           // IRQ - read the VIC register
//		LDR	  pc,_irq					// or go to default handler
//        MOV   pc,#__fiq // Do __fiq in-place instead
__fiq:  MOV r0,#_fiqstr                  // FIQ
		B _printexc
__undf: MOV r0,#_undfstr                 // undefined
		B _printexc
__pabt: MOV r0,#_pabtstr                 // program abort
		B _printexc
__dabt: MOV r0,#_dabtstr                 // data abort
		// lr contains instruction that failed to access data +8 so adjust an additional 4 bytes
		sub lr,lr,#4
		B _printexc
__swi:
		MOV r0,#_swistr
//		B _printexc // Fall-thru
_printexc:
		.set	UART0,	0xe000c000
		.set	U0LSR_OFFS, 0x14
		.set	U0IER_OFFS, 0x04
		.set	U0THR_OFFS, 0x00
		.set	FIO0CLR, 0x3fffc01c
		// lr contains address of failed instruction +4
		SUB lr,lr,#4
		LDR r1,=UART0
//		MOV r2,#0
//		STR r2,[r1,#U0IER_OFFS]

		// Output exception text
_ploop:
		LDRB r2,[r0],#1
_pwait:
		LDR r3,[r1,#U0LSR_OFFS]
		TST r3,#0x20
		BEQ _pwait
		CMP r2,#0
		STRNE r2,[r1,#U0THR_OFFS]
		BNE _ploop
_done:

		// Output lr register in hex
		MOV r5,#8
		MOV r4,#_hexstr
_hloop:
		LSR r0,lr,#0x1c
		LDR r2,[r4,r0]
_pwait2:
		LDR r3,[r1,#U0LSR_OFFS]
		TST r3,#0x20
		BEQ _pwait2
		STR r2,[r1,#U0THR_OFFS]
		LSL lr,lr,#4
		SUBS r5,r5,#1
		BNE _hloop

		// Turn off backlight to indicate fault
//		LDR r1,=FIO0CLR
//		MOV r2,#(1<<11)
//		STR r2,[r1]
_realdone:
		B _realdone

_hexstr: .ASCII "0123456789abcdef"
_undfstr: .ASCIZ "UNDF@"
_pabtstr: .ASCIZ "PABT@"
_dabtstr: .ASCIZ "DABT@"
_fiqstr: .ASCIZ "FIQ?"
_swistr: .ASCIZ "SWI?"

		.endfunc
/*
 * Setup the operating mode & stack.
 */
        .global _start, start, _mainCRTStartup
        .func   _start

_start:
start:
_mainCRTStartup:

		.set	SYSCTRL,			0xE01FC040

#ifdef PLL_INIT
		
		.set	PLL,				SYSCTRL+0x40
		.set	PLLCON_OFFSET,		0x0
		.set	PLLCFG_OFFSET,		0x4
		.set	PLLSTAT_OFFSET,		0x8
		.set	PLLFEED_OFFSET,		0xC
		
		.set	PLOCK,				(1<<10)	//lock bit inside PLLSTAT

		.set	SET_PLLCFG_MUL3,	0x2
		.set	SET_PLLCFG_MUL4,	0x3
		.set	SET_PLLCFG_MUL5,	0x4
		.set	SET_PLLCFG_MUL6,	0x5
		
		.set	SET_PLLCFG_DIV1,	(0x0<<4)
		.set	SET_PLLCFG_DIV2,	(0x1<<4)
		.set	SET_PLLCFG_DIV4,	(0x2<<4)
		.set	SET_PLLCFG_DIV8,	(0x3<<4)
		

		.set	PLLCFG_INIT_VAL,	SET_PLLCFG_MUL6 | SET_PLLCFG_DIV1
		.set	SET_PLLCON_ENABLE,	1
		.set	SET_PLLCON_CONNECT,	2
	
/*
 * Setup the PLL
 */

		LDR		R0,=PLL
		MOV		R1,#0xAA
		MOV		R2,#0x55

		MOV		R3,#PLLCFG_INIT_VAL
		STR		R3,[R0,#PLLCFG_OFFSET]

		MOV		R3,#SET_PLLCON_ENABLE
		STR		R3,[R0,#PLLCON_OFFSET]
		
		STR		R1,[R0,#PLLFEED_OFFSET]
		STR		R2,[R0,#PLLFEED_OFFSET]
		
		// Wait for the loop to lock
1:		LDR		R3,[R0,#PLLSTAT_OFFSET]
		ANDS	R3,R3,#PLOCK
		BEQ		1b
		
		// Now swap the cpu clock to the PLL 
		MOV		R3,#(SET_PLLCON_ENABLE | SET_PLLCON_CONNECT)
		STR		R3,[R0,#PLLCON_OFFSET]
		STR		R1,[R0,#PLLFEED_OFFSET]
		STR		R2,[R0,#PLLFEED_OFFSET]

#endif



#ifdef VPB_INIT
/*
 * Setup the VPB/APB Peripheral bus clock
 */

		.set	VPBDIV_OFFSET,		0xc0
		.set	VPBDIV,				SYSCTRL+VPBDIV_OFFSET
		
		.set	VPBDIV_INIT_VAL,	1
		
		LDR		R0,=VPBDIV
		LDR		R1,=VPBDIV_INIT_VAL
		STR		R1,[R0]
		
#endif
		
		
#ifdef MAM_INIT
/*
 * Setup the Memory Accelerator Block (MAM)
 */
	
		.set	MAM,				0xE01FC000
		.set	MAMCR_OFFSET,		0x0
		.set	MAMTIM_OFFSET,		0x4

		.set	SET_MAMCR_DISABLE,	0x0
		.set	SET_MAMCR_PARTIAL,	0x1
		.set	SET_MAMCR_FULL,		0x2

		// How many cycles for flash access		
		.set	SET_MAMTIM_0CLK,	0x0
		.set	SET_MAMTIM_1CLK,	0x1
		.set	SET_MAMTIM_2CLK,	0x2
		.set	SET_MAMTIM_3CLK,	0x3
		.set	SET_MAMTIM_4CLK,	0x4
		.set	SET_MAMTIM_5CLK,	0x5
		.set	SET_MAMTIM_6CLK,	0x6
		.set	SET_MAMTIM_7CLK,	0x7
		
		
		.set	MAMTIM_INIT_VAL,	SET_MAMTIM_3CLK
		.set	MAMCR_INIT_VAL,		SET_MAMCR_FULL
		
		LDR		R0,=MAM
		MOV		R1,#MAMTIM_INIT_VAL
		STR		R1,[R0,#MAMTIM_OFFSET]
		MOV		R1,#MAMCR_INIT_VAL
		STR		R1,[R0,#MAMCR_OFFSET]

#endif

/***************************
 * The LPC2146/2148 have 8KB of USB RAM available. If USB is
 * not being used, then this RAM can be used as general
 * purpose RAM by the application. However this requires the
 * USB subsystem to be enabled in the PCONP register
 ***************************/
#ifdef USB_INIT
		LDR r0, =0xE01FC0C4	// Load address of PCONP register
		LDR r1,[r0]			// Load contents of PCONP
		ORR r1,r1,#(1 << 31)	// Set bit 31 - USB
		STR r1,[r0]			// Store updated value back to PCONP
#endif

/*
 * Setup some stack space for each ARM operating mode
 */
        LDR   r0,=_vStackTop
        MSR   CPSR_c,#MODE_UND|I_BIT|F_BIT // Undefined Instruction Mode
        MOV   sp,r0
        SUB   r0,r0,#UND_STACK_SIZE
        MSR   CPSR_c,#MODE_ABT|I_BIT|F_BIT // Abort Mode
        MOV   sp,r0
        SUB   r0,r0,#ABT_STACK_SIZE
        MSR   CPSR_c,#MODE_FIQ|I_BIT|F_BIT // FIQ Mode
        MOV   sp,r0
        SUB   r0,r0,#FIQ_STACK_SIZE
        MSR   CPSR_c,#MODE_IRQ|I_BIT|F_BIT // IRQ Mode
        MOV   sp,r0
        SUB   r0,r0,#IRQ_STACK_SIZE
        MSR   CPSR_c,#MODE_SVC|I_BIT|F_BIT // Supervisor Mode
        MOV   sp,r0
        SUB   r0,r0,#SVC_STACK_SIZE
        MSR   CPSR_c,#MODE_SYS|I_BIT|F_BIT	// System Mode
        MOV   sp,r0

#ifndef USE_OLD_STYLE_DATA_BSS_INIT
/*
 * Copy RWdata initial values from flash to its execution
 * address in RAM
 */
		LDR	r4, =Ldata_start	// Load base address of data...
		LDR r4, [r4]			// ...from Global Section Table
		LDR r5, =Ldata_end		// Load end address of data...
		LDR r5, [r5]			//...from Global Section Table
start_data_init_loop:
		CMP	r4,r5				// Check to see if reached end of...
		BEQ	end_data_init_loop	// ...data entries in G.S.T.
		LDR r0, [r4],#4			// Load LoadAddr from G.S.T.
		LDR r1, [r4],#4			// Load ExeAddr from G.S.T.
		LDR r2, [r4],#4			// Load SectionLen from G.S.T.
		BL	data_init			// Call subroutine to do copy
		B start_data_init_loop	// Loop back for next entry in G.S.T.

end_data_init_loop:
/*
 * Clear .bss (zero'ed space)
 */
		LDR r5, =Lbss_end		// Load end address of BSS...
		LDR r5, [r5]			//...from Global Section Table
start_bss_init_loop:
		CMP	r4,r5				// Check to see if reached end of...
		BEQ	post_data_bss_init	// ...bss entries in G.S.T.
		LDR r0, [r4],#4			// Load ExeAddr from G.S.T.
		LDR r1, [r4],#4			// Load SectionLen from G.S.T.
		BL	bss_init			// Call subroutine to do zero'ing
		B	start_bss_init_loop	// Loop back for next entry in G.S.T.

Ldata_start:   .word __data_section_table
Ldata_end:     .word __data_section_table_end
Lbss_end:      .word __bss_section_table_end

/******************************************************************************
 * Functions to carry out the initialization of RW and BSS data sections. These
 * are written as separate functions to cope with MCUs with multiple banks of
 * memory.
 ******************************************************************************/
// void data_init(unsigned int romstart, unsigned int start, unsigned int len)
data_init:
		MOV	r12,#0
.di_loop:
		CMP	r12,r2
        LDRLO 	r3,[r0],#4
        STRLO 	r3,[r1],#4
		ADDLO	r12,r12,#4
		BLO	.di_loop
		BX LR

// void bss_init(unsigned int start, unsigned int len)
bss_init:
		MOV	r12,#0
		MOV r2, #0
.bi_loop:
		CMP	r12,r1
        STRLO 	r2,[r0],#4
		ADDLO	r12,r12,#4
		BLO	.bi_loop
		BX LR


/******************************************************************************
 * Back to main flow of Reset_Handler
 ******************************************************************************/
post_data_bss_init:

#else
	// Use Old Style Data and BSS section initialization.
	// This will only initialize a single RAM bank
	//
	// Copy initialized data to its execution address in RAM
        LDR   r1,=_etext                // -> ROM data start
        LDR   r2,=_data                 // -> data start
        LDR   r3,=_edata                // -> end of data
1:      CMP   r2,r3                     // check if data to move
        LDRLO r0,[r1],#4                // copy it
        STRLO r0,[r2],#4
        BLO   1b                        // loop until done

	//Clear .bss (zero'ed space)
        MOV   r0,#0                     // get a zero
        LDR   r1,=_bss                  // -> bss start
        LDR   r2,=_ebss                 // -> bss end
2:      CMP   r1,r2                     // check if data to clear
        STRLO r0,[r1],#4                // clear 4 bytes
        BLO   2b                        // loop until done
#endif

#ifdef STACK_INIT
/*
 * Initialize the stack to known values to aid debugging
 *
 * Definitely optional, but can help early debugging for
 * stack overflows. Also system optimization for measuring
 * stack depth used.
 */
		.global _vStackTop
		
		MOV		r0,#0					// start of count
		LDR		r1,=_vStackTop			// start from top
                LDR r3,=TOTAL_STACK_SIZE
                SUB r2,r1,r3
3:
		CMP		r1,r2
		STRGT	r0,[r1,#-4]!				// walk down the stack
		ADD		r0,r0,#1
		BGT		3b
#endif

       //
       // Call C++ library initilisation, if present
       //
.cpp_init:
        LDR   r3, .def__libc_init_array  // if
        CMP   r3, #0
        BEQ   .skip_cpp_init
        BL    __libc_init_array
.skip_cpp_init:

/*
 * Call main program: main(0)
 */
        MOV   r0,#0                     // no arguments (argc = 0)
//       MOV   r1,r0
//       MOV   r2,r0
        MOV   fp,r0                     // null frame pointer
//        MOV   r7,r0                     // null frame pointer for thumb

		// Change to system mode (IRQs enabled) before calling main application

        MSR   CPSR_c,#MODE_SYS|F_BIT	// System Mode

#ifdef __REDLIB__
        LDR r10, =__main
#else
        LDR r10, =main
#endif

        MOV   lr,pc
        BX    r10                       // enter main() - could be ARM or Thumb

        .size   _start, . - _start
        .endfunc

        .global _reset, reset
        .func   _reset
_reset:
reset:
exit:

        B     .                         // loop until reset

	.weak   __libc_init_array
.def__libc_init_array:
	.word   __libc_init_array


        .weak 	init_lpc3xxx			// void init_lpc31xx(void)
.def__init_lpc3xxx:
		.word	init_lpc3xxx


        .size _reset, . - _reset
        .endfunc

        .end
